//
// Created by 16933 on 2024/1/11.
//

#include "Control.h"
#include "cmsis_os.h"
#include "gpio.h"
#include "tim.h"
#include "bsp_MPU6050.h"
#include "Message.h"
int angle = 0;
//MomentumWheel_Controler momentumwell;

Motor_Controller motor;
Posture posture;
extern Message blutooth_message;
extern osSemaphoreId ControlBinarySemHandle;
void ControlTask(void const * argument)
{
    /* USER CODE BEGIN ControlTask */
    HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_2);
    HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_1);
    HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_1);
    MomentumWheel_Enable();
    uint32_t Contiol_time = osKernelSysTick();
    /* Infinite loop */
    for(;;)
    {
        // 等待信号量
        xSemaphoreTake(ControlBinarySemHandle, portMAX_DELAY);
        if (message_state != MPU_INIT_ERROR)
        {
            if (motor.momentum_weel.diraction == CLOCKWISE)
            {
                MomentumWheel_Clockwise(motor.momentum_weel.pwm_duty);
            }
            else
            {
                MomentumWheel_Counterclockwise(motor.momentum_weel.pwm_duty);
            }
            //前轮转向
            motor.turn_weel.trun_angle = blutooth_message.turn;
            Servo_Control(&motor.turn_weel);
            //后轮驱动

            motor.drive_weel.speed = motor.drive_weel.blutooth_flag * motor.drive_weel.speed_bluetooth + blutooth_message.forward-blutooth_message.backward;
            DriveWeel_Control(&motor.drive_weel);
            osDelayUntil(&Contiol_time,1);
        }

    }
    /* USER CODE END ControlTask */
}

void MomentumWheel_Disable()
{
    HAL_GPIO_WritePin(MomentumWheel_EN_GPIO_Port,MomentumWheel_EN_Pin,GPIO_PIN_RESET);
}

/**
 * @brief 使能动量轮
 * **/
void MomentumWheel_Enable()
{
        HAL_GPIO_WritePin(MomentumWheel_EN_GPIO_Port,MomentumWheel_EN_Pin,GPIO_PIN_SET);
}
/**
 * @param duty:占空比 最大为 2000
 *
 * */
void MomentumWheel_Clockwise(int duty)
{

//    HAL_GPIO_WritePin(MomentumWheel_EN_GPIO_Port,MomentumWheel_EN_Pin,GPIO_PIN_SET);
    HAL_GPIO_WritePin(MomentumWheel_DIR_GPIO_Port,MomentumWheel_DIR_Pin,GPIO_PIN_SET);
    __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_1,duty);
}

/**
 * @param duty:占空比 最大为 2000
 * */
void MomentumWheel_Counterclockwise(int duty)
{
//    HAL_GPIO_WritePin(MomentumWheel_EN_GPIO_Port,MomentumWheel_EN_Pin,GPIO_PIN_SET);

    HAL_GPIO_WritePin(MomentumWheel_DIR_GPIO_Port,MomentumWheel_DIR_Pin,GPIO_PIN_RESET);
    __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_1,duty);
}
//
//void DriveWeel_Forward(int duty)
//{
//    HAL_GPIO_WritePin(DriveWheel_STBY_GPIO_Port,DriveWheel_STBY_Pin,GPIO_PIN_SET);
//    if (duty >= 0)
//    {
//        HAL_GPIO_WritePin(DriveWheel_AIN2_GPIO_Port,DriveWheel_AIN2_Pin,GPIO_PIN_RESET);
//        HAL_GPIO_WritePin(DriveWheel_AIN1_GPIO_Port,DriveWheel_AIN1_Pin,GPIO_PIN_SET);
//        __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_2,duty);
//
//    }
//    else
//    {
//        HAL_GPIO_WritePin(DriveWheel_AIN1_GPIO_Port,DriveWheel_AIN1_Pin,GPIO_PIN_RESET);
//        HAL_GPIO_WritePin(DriveWheel_AIN2_GPIO_Port,DriveWheel_AIN2_Pin,GPIO_PIN_SET);
//        __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_2,duty);
//    }
//}
////
void DriveWeel_Control(DriveWeel_Controler *drive_motor)
{
    HAL_GPIO_WritePin(DriveWheel_STBY_GPIO_Port,DriveWheel_STBY_Pin,GPIO_PIN_SET);
    if (drive_motor->speed >= 0)
    {
        drive_motor->diraction = FORWORD;
        drive_motor->pwm_duty = (int)drive_motor->speed;
        HAL_GPIO_WritePin(DriveWheel_AIN2_GPIO_Port,DriveWheel_AIN2_Pin,GPIO_PIN_RESET);
        HAL_GPIO_WritePin(DriveWheel_AIN1_GPIO_Port,DriveWheel_AIN1_Pin,GPIO_PIN_SET);
        __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_2,drive_motor->pwm_duty);
    }
    else
    {
        drive_motor->diraction = BACKWORD;
        drive_motor->pwm_duty = -(int)drive_motor->speed;
        HAL_GPIO_WritePin(DriveWheel_AIN1_GPIO_Port,DriveWheel_AIN1_Pin,GPIO_PIN_RESET);
        HAL_GPIO_WritePin(DriveWheel_AIN2_GPIO_Port,DriveWheel_AIN2_Pin,GPIO_PIN_SET);
        __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_2,drive_motor->pwm_duty);
    }
}

void Servo_Control(Servor_Controler *servor_motor)
{
    servor_motor->pwm_duty = (int)(servor_motor->trun_angle/9);
    if (servor_motor->pwm_duty > 9)
    {
        servor_motor->pwm_duty = 9;
    }

    if (servor_motor->trun_angle>0)
    {
        servor_motor->diraction = SERVO_TURN_RIGHT;
        __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1,MID_PWM_DUTY-servor_motor->pwm_duty);
    }
    else if(servor_motor->trun_angle<0)
    {
        servor_motor->diraction = SERVO_TURN_LEFT;
        __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1,MID_PWM_DUTY+servor_motor->pwm_duty);
    }
    else
    {
        __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1,MID_PWM_DUTY);
    }

}